// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: compute_mag_innov_innov_var_and_hx
 *
 * Args:
 *     state: Matrix24_1
 *     P: Matrix24_24
 *     meas: Matrix31
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov: Matrix31
 *     innov_var: Matrix31
 *     Hx: Matrix24_1
 */
template <typename Scalar>
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
                                  const matrix::Matrix<Scalar, 24, 24>& P,
                                  const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
                                  const Scalar epsilon,
                                  matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
                                  matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
                                  matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
  // Total ops: 471

  // Input arrays

  // Intermediate terms (49)
  const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2));
  const Scalar _tmp1 = -2 * std::pow(state(2, 0), Scalar(2));
  const Scalar _tmp2 = _tmp0 + _tmp1 + 1;
  const Scalar _tmp3 = 2 * state(0, 0);
  const Scalar _tmp4 = _tmp3 * state(3, 0);
  const Scalar _tmp5 = 2 * state(2, 0);
  const Scalar _tmp6 = _tmp5 * state(1, 0);
  const Scalar _tmp7 = _tmp4 + _tmp6;
  const Scalar _tmp8 = _tmp5 * state(0, 0);
  const Scalar _tmp9 = 2 * state(1, 0);
  const Scalar _tmp10 = _tmp9 * state(3, 0);
  const Scalar _tmp11 = _tmp10 - _tmp8;
  const Scalar _tmp12 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
  const Scalar _tmp13 = _tmp0 + _tmp12;
  const Scalar _tmp14 = _tmp5 * state(3, 0);
  const Scalar _tmp15 = _tmp3 * state(1, 0);
  const Scalar _tmp16 = _tmp14 + _tmp15;
  const Scalar _tmp17 = -_tmp4 + _tmp6;
  const Scalar _tmp18 = _tmp1 + _tmp12;
  const Scalar _tmp19 = _tmp14 - _tmp15;
  const Scalar _tmp20 = _tmp10 + _tmp8;
  const Scalar _tmp21 = _tmp9 * state(18, 0);
  const Scalar _tmp22 = _tmp3 * state(17, 0);
  const Scalar _tmp23 = _tmp21 + _tmp22 - 4 * state(16, 0) * state(3, 0);
  const Scalar _tmp24 = 4 * state(2, 0);
  const Scalar _tmp25 = _tmp9 * state(17, 0);
  const Scalar _tmp26 = _tmp3 * state(18, 0);
  const Scalar _tmp27 = -_tmp24 * state(16, 0) + _tmp25 - _tmp26;
  const Scalar _tmp28 = state(17, 0) * state(3, 0);
  const Scalar _tmp29 = 2 * _tmp28;
  const Scalar _tmp30 = _tmp5 * state(18, 0);
  const Scalar _tmp31 = _tmp29 - _tmp30;
  const Scalar _tmp32 = 2 * state(3, 0);
  const Scalar _tmp33 = _tmp32 * state(18, 0);
  const Scalar _tmp34 = _tmp5 * state(17, 0);
  const Scalar _tmp35 = _tmp33 + _tmp34;
  const Scalar _tmp36 = _tmp5 * state(16, 0);
  const Scalar _tmp37 = 4 * state(1, 0);
  const Scalar _tmp38 = _tmp26 + _tmp36 - _tmp37 * state(17, 0);
  const Scalar _tmp39 = _tmp3 * state(16, 0);
  const Scalar _tmp40 = -4 * _tmp28 + _tmp30 - _tmp39;
  const Scalar _tmp41 = _tmp32 * state(16, 0);
  const Scalar _tmp42 = _tmp21 - _tmp41;
  const Scalar _tmp43 = _tmp9 * state(16, 0);
  const Scalar _tmp44 = _tmp33 + _tmp43;
  const Scalar _tmp45 = -_tmp22 - _tmp37 * state(18, 0) + _tmp41;
  const Scalar _tmp46 = -_tmp24 * state(18, 0) + _tmp29 + _tmp39;
  const Scalar _tmp47 = _tmp34 + _tmp43;
  const Scalar _tmp48 = -_tmp25 + _tmp36;

  // Output terms (3)
  if (innov != nullptr) {
    matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);

    _innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) -
                   meas(0, 0) + state(19, 0);
    _innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) -
                   meas(1, 0) + state(20, 0);
    _innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) -
                   meas(2, 0) + state(21, 0);
  }

  if (innov_var != nullptr) {
    matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);

    _innov_var(0, 0) =
        P(0, 19) * _tmp31 + P(1, 19) * _tmp35 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
        P(18, 19) * _tmp11 + P(19, 19) + P(2, 19) * _tmp27 + P(3, 19) * _tmp23 + R +
        _tmp11 * (P(0, 18) * _tmp31 + P(1, 18) * _tmp35 + P(16, 18) * _tmp2 + P(17, 18) * _tmp7 +
                  P(18, 18) * _tmp11 + P(19, 18) + P(2, 18) * _tmp27 + P(3, 18) * _tmp23) +
        _tmp2 * (P(0, 16) * _tmp31 + P(1, 16) * _tmp35 + P(16, 16) * _tmp2 + P(17, 16) * _tmp7 +
                 P(18, 16) * _tmp11 + P(19, 16) + P(2, 16) * _tmp27 + P(3, 16) * _tmp23) +
        _tmp23 * (P(0, 3) * _tmp31 + P(1, 3) * _tmp35 + P(16, 3) * _tmp2 + P(17, 3) * _tmp7 +
                  P(18, 3) * _tmp11 + P(19, 3) + P(2, 3) * _tmp27 + P(3, 3) * _tmp23) +
        _tmp27 * (P(0, 2) * _tmp31 + P(1, 2) * _tmp35 + P(16, 2) * _tmp2 + P(17, 2) * _tmp7 +
                  P(18, 2) * _tmp11 + P(19, 2) + P(2, 2) * _tmp27 + P(3, 2) * _tmp23) +
        _tmp31 * (P(0, 0) * _tmp31 + P(1, 0) * _tmp35 + P(16, 0) * _tmp2 + P(17, 0) * _tmp7 +
                  P(18, 0) * _tmp11 + P(19, 0) + P(2, 0) * _tmp27 + P(3, 0) * _tmp23) +
        _tmp35 * (P(0, 1) * _tmp31 + P(1, 1) * _tmp35 + P(16, 1) * _tmp2 + P(17, 1) * _tmp7 +
                  P(18, 1) * _tmp11 + P(19, 1) + P(2, 1) * _tmp27 + P(3, 1) * _tmp23) +
        _tmp7 * (P(0, 17) * _tmp31 + P(1, 17) * _tmp35 + P(16, 17) * _tmp2 + P(17, 17) * _tmp7 +
                 P(18, 17) * _tmp11 + P(19, 17) + P(2, 17) * _tmp27 + P(3, 17) * _tmp23);
    _innov_var(1, 0) =
        P(0, 20) * _tmp42 + P(1, 20) * _tmp38 + P(16, 20) * _tmp17 + P(17, 20) * _tmp13 +
        P(18, 20) * _tmp16 + P(2, 20) * _tmp44 + P(20, 20) + P(3, 20) * _tmp40 + R +
        _tmp13 * (P(0, 17) * _tmp42 + P(1, 17) * _tmp38 + P(16, 17) * _tmp17 + P(17, 17) * _tmp13 +
                  P(18, 17) * _tmp16 + P(2, 17) * _tmp44 + P(20, 17) + P(3, 17) * _tmp40) +
        _tmp16 * (P(0, 18) * _tmp42 + P(1, 18) * _tmp38 + P(16, 18) * _tmp17 + P(17, 18) * _tmp13 +
                  P(18, 18) * _tmp16 + P(2, 18) * _tmp44 + P(20, 18) + P(3, 18) * _tmp40) +
        _tmp17 * (P(0, 16) * _tmp42 + P(1, 16) * _tmp38 + P(16, 16) * _tmp17 + P(17, 16) * _tmp13 +
                  P(18, 16) * _tmp16 + P(2, 16) * _tmp44 + P(20, 16) + P(3, 16) * _tmp40) +
        _tmp38 * (P(0, 1) * _tmp42 + P(1, 1) * _tmp38 + P(16, 1) * _tmp17 + P(17, 1) * _tmp13 +
                  P(18, 1) * _tmp16 + P(2, 1) * _tmp44 + P(20, 1) + P(3, 1) * _tmp40) +
        _tmp40 * (P(0, 3) * _tmp42 + P(1, 3) * _tmp38 + P(16, 3) * _tmp17 + P(17, 3) * _tmp13 +
                  P(18, 3) * _tmp16 + P(2, 3) * _tmp44 + P(20, 3) + P(3, 3) * _tmp40) +
        _tmp42 * (P(0, 0) * _tmp42 + P(1, 0) * _tmp38 + P(16, 0) * _tmp17 + P(17, 0) * _tmp13 +
                  P(18, 0) * _tmp16 + P(2, 0) * _tmp44 + P(20, 0) + P(3, 0) * _tmp40) +
        _tmp44 * (P(0, 2) * _tmp42 + P(1, 2) * _tmp38 + P(16, 2) * _tmp17 + P(17, 2) * _tmp13 +
                  P(18, 2) * _tmp16 + P(2, 2) * _tmp44 + P(20, 2) + P(3, 2) * _tmp40);
    _innov_var(2, 0) =
        P(0, 21) * _tmp48 + P(1, 21) * _tmp45 + P(16, 21) * _tmp20 + P(17, 21) * _tmp19 +
        P(18, 21) * _tmp18 + P(2, 21) * _tmp46 + P(21, 21) + P(3, 21) * _tmp47 + R +
        _tmp18 * (P(0, 18) * _tmp48 + P(1, 18) * _tmp45 + P(16, 18) * _tmp20 + P(17, 18) * _tmp19 +
                  P(18, 18) * _tmp18 + P(2, 18) * _tmp46 + P(21, 18) + P(3, 18) * _tmp47) +
        _tmp19 * (P(0, 17) * _tmp48 + P(1, 17) * _tmp45 + P(16, 17) * _tmp20 + P(17, 17) * _tmp19 +
                  P(18, 17) * _tmp18 + P(2, 17) * _tmp46 + P(21, 17) + P(3, 17) * _tmp47) +
        _tmp20 * (P(0, 16) * _tmp48 + P(1, 16) * _tmp45 + P(16, 16) * _tmp20 + P(17, 16) * _tmp19 +
                  P(18, 16) * _tmp18 + P(2, 16) * _tmp46 + P(21, 16) + P(3, 16) * _tmp47) +
        _tmp45 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp45 + P(16, 1) * _tmp20 + P(17, 1) * _tmp19 +
                  P(18, 1) * _tmp18 + P(2, 1) * _tmp46 + P(21, 1) + P(3, 1) * _tmp47) +
        _tmp46 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp45 + P(16, 2) * _tmp20 + P(17, 2) * _tmp19 +
                  P(18, 2) * _tmp18 + P(2, 2) * _tmp46 + P(21, 2) + P(3, 2) * _tmp47) +
        _tmp47 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp45 + P(16, 3) * _tmp20 + P(17, 3) * _tmp19 +
                  P(18, 3) * _tmp18 + P(2, 3) * _tmp46 + P(21, 3) + P(3, 3) * _tmp47) +
        _tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp45 + P(16, 0) * _tmp20 + P(17, 0) * _tmp19 +
                  P(18, 0) * _tmp18 + P(2, 0) * _tmp46 + P(21, 0) + P(3, 0) * _tmp47);
  }

  if (Hx != nullptr) {
    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);

    _hx.setZero();

    _hx(0, 0) = _tmp31;
    _hx(1, 0) = _tmp35;
    _hx(2, 0) = _tmp27;
    _hx(3, 0) = _tmp23;
    _hx(16, 0) = _tmp2;
    _hx(17, 0) = _tmp7;
    _hx(18, 0) = _tmp11;
    _hx(19, 0) = 1;
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
